1. Field of the Invention
The present invention generally relates to a method and system for efficiently finding a surface.
2. Description of the Related Art
It is useful for robots to find supporting surfaces, such as tables and floors, in their environment. Deviations from these surfaces can then be interpreted as obstacles to navigate around (e.g., on the floor) or possible objects to manipulate (e.g., on a table). The navigation aspect also applies to self-guided vehicles on roads or in warehouses.
One popular solution is to obtain a 3D (3-dimensional) point cloud from a sensor such as Microsoft's Kinect®. As shown in FIG. 1, a set of three points is chosen which uniquely defines a plane on which they all rest. This is repeated for another set of three points, and another, etc. in a RANSAC (random sample consensus) manner. The point triads then vote for a plane in a Hough space for a set of parameters such as tilt, roll, and height of the plane. From the vote densities and various other constraints (e.g., expected plane height), the “best” plane parameter set is found. Based on this, deviations in the scene can be assessed.
However, this method requires a large amount of computation. For a Video Graphics Array (VGA) resolution depth image there may be n=300,000 points. There are roughly n3 combinations of points that could be chosen, or roughly 2×1016 sets! This could be reduced to, say, 1% of all points by stochastic sampling, or by dividing the image into 10×10 pixel grid cells and selecting a single point from each cell. However, this still leaves 2×1010 triads, which can take a long time to evaluate. Yet the combinations cannot be reduced much more without fear of missing a plane with a small surface size.
Another approach would be to take the 3D convex hull of the point cloud as illustrated in FIGS. 2a-2c. This can be done reasonably efficiently in n*log n time (or n*log h, where h is the number of points on the hull). The facets of the hull are then candidate planes. Alternatively, a mesh reduction and approximation scheme can be used to consolidate similar patches on the hull into larger facets (planes). The resulting set of facets is then examined, possibly in light of further external constraints, to find the surface of interest. Once again, from the selected surface the remainder of the original depth image can be evaluated to find deviations from the plane.
Unfortunately, the convex hull algorithm requires that the plane be a true extrema in the environment. For instance, gazing from a landing down a flight of stairs would return a “fictitious” surface that followed the creases in the steps.
Still another method for finding the floor, or another flat surface, is by direct plane sweeping. As illustrated in FIGS. 3a and 3b, one guesses a plane and then moves it through the point cloud accumulating evidence. A threshold can be set so that small regions still contribute to the evidence for a particular plane. However, the method is very time consuming because there are a very large number of offsets and orientations to try.